/**
 * @author Yunfeng Bai
 *
 * 2010-12-1
 */
package main;

import geometry.Box;
import geometry.Triangle;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;

import javax.vecmath.Color3f;
import javax.vecmath.Matrix4d;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

import rigidbody.BoxRigidBody;
import rigidbody.RigidBody;
import rigidbody.SimulationObject;
import simulation.ContactGraph;
import simulation.ProjectedGaussSeidel;
import simulation.ProjectedGaussSeidel.PGSResult;
import simulation.ProjectedGaussSeidelConstraintSolver;
import util.Util;
import cern.colt.matrix.impl.DenseDoubleMatrix1D;
import cern.colt.matrix.impl.DenseDoubleMatrix2D;
import cern.colt.matrix.linalg.EigenvalueDecomposition;
import cern.jet.math.Functions;
import collision.Collision;
import collision.OBB;

public class Test {
	public void testAll()
	{
//		testProjection();
//		testDynamics();
//		testGaussSeidel1();
		testGaussSeidel2();
//		testOBBIntersection();
//		testTriangleDistance();
//		testInsideCheck();
//		testPenetrationDetection();
//		testASDF();
//		testContactGraph();
//		testBoxIntersectWithRay();
	}
	
	private void testProjection()
	{
		RigidBody boxBody = new BoxRigidBody(10, new Point3d(0, 0.5, 0), new Vector3d(2, 2, 2));
    	RigidBody table = new BoxRigidBody(10, new Point3d(0, -1, 0), new Vector3d(10, 2, 10));
    	table.setIsImmobile(true);
    	List<RigidBody> bodies = new ArrayList<RigidBody>();
    	bodies.add(boxBody);
    	bodies.add(table);
    	
		List<Collision> collisions = new ArrayList<Collision>();
		collisions.add(new Collision(0.5, boxBody, table, new Point3d(1, -0.5, 1), new Vector3d(0, -1, 0)));
		collisions.add(new Collision(0.5, boxBody, table, new Point3d(1, -0.5, -1), new Vector3d(0, -1, 0)));
		collisions.add(new Collision(0.5, boxBody, table, new Point3d(-1, -0.5, 1), new Vector3d(0, -1, 0)));
		collisions.add(new Collision(0.5, boxBody, table, new Point3d(-1, -0.5, -1), new Vector3d(0, -1, 0)));
		
		ProjectedGaussSeidelConstraintSolver solver = new ProjectedGaussSeidelConstraintSolver();
		solver.setBodies(bodies);
		solver.setTimestep(1);
		solver.setCollisions(collisions);
		solver.solveProjectionWithCollisions();
		SimulationObject.ResetNextId();
	}
	
	private void testDynamics()
	{
		RigidBody boxBody = new BoxRigidBody(10, new Point3d(0, 1, 0), new Vector3d(2, 2, 2));
    	RigidBody table = new BoxRigidBody(10, new Point3d(0, -1, 0), new Vector3d(10, 2, 10));
    	table.setIsImmobile(true);
    	List<RigidBody> bodies = new ArrayList<RigidBody>();
    	bodies.add(boxBody);
    	bodies.add(table);
    	
		List<Collision> collisions = new ArrayList<Collision>();
		collisions.add(new Collision(0.1, boxBody, table, new Point3d(1, 0, 1), new Vector3d(0, -1, 0)));
		collisions.add(new Collision(0.1, boxBody, table, new Point3d(1, 0, -1), new Vector3d(0, -1, 0)));
		collisions.add(new Collision(0.1, boxBody, table, new Point3d(-1, 0, 1), new Vector3d(0, -1, 0)));
		collisions.add(new Collision(0.1, boxBody, table, new Point3d(-1, 0, -1), new Vector3d(0, -1, 0)));
		
		boxBody.accumulateForce(new Vector3d(0, -10, 0));
		
		ProjectedGaussSeidelConstraintSolver solver = new ProjectedGaussSeidelConstraintSolver();
		solver.setBodies(bodies);
		solver.setTimestep(1);
		solver.setCollisions(collisions);
		solver.solveDynamicsWithCollisions();
		SimulationObject.ResetNextId();
		
		SimulationObject.ResetNextId();
	}
	
	private void testGaussSeidel1()
	{
		ProjectedGaussSeidel psg = new ProjectedGaussSeidel();
		double[][] ca = new double[2][];
		ca[0] = new double[] {16, 3};
		ca[1] = new double[] {7, -11};
		double[] cb = new double[] {-11, -13};
		double[] clo = new double[] {-Double.MAX_VALUE, -Double.MAX_VALUE};
		double[] chi = new double[] {Double.MAX_VALUE, Double.MAX_VALUE};
		DenseDoubleMatrix2D a = new DenseDoubleMatrix2D(ca);
		DenseDoubleMatrix1D b = new DenseDoubleMatrix1D(cb);
		DenseDoubleMatrix1D lo = new DenseDoubleMatrix1D(clo);
		DenseDoubleMatrix1D hi = new DenseDoubleMatrix1D(chi);
		int maxIteration = 10;
		double threshold = util.Constants.FP_TOLERANCE;
		PGSResult result = psg.solve(a, b, lo, hi, maxIteration, threshold, false);
		DenseDoubleMatrix1D w = new DenseDoubleMatrix1D(2);
		a.zMult(result.result, w);
		w.assign(b, Functions.plus);
		assert(Math.abs(w.get(0)) < 1e-8);
		assert(Math.abs(w.get(1)) < 1e-8);
	}
	
	private void testGaussSeidel2()
	{
		ProjectedGaussSeidel psg = new ProjectedGaussSeidel();
		double[][] ca = new double[4][];
		ca[0] = new double[] {0.164103, 0.160497, 0.148234, 0.152086};
		ca[1] = new double[] {0.160497, 0.163053, 0.150272, 0.160497};
		ca[2] = new double[] {0.148234, 0.150272, 0.140082, 0.148234};
		ca[3] = new double[] {0.152086, 0.160497, 0.148234, 0.164002};
//		ca[0] = new double[] {0.7555, 0.6384, 0.5799, 0.5789};
//		ca[1] = new double[] {0.6384, 0.5436, 0.4922, 0.5168};
//		ca[2] = new double[] {0.5799, 0.4922, 0.4463, 0.4592};
//		ca[3] = new double[] {0.5789, 0.5168, 0.4592, 0.6316};
		double[] cb = new double[] {-1, -2, -3, -4};
		double[] clo = new double[] {0, 0, 0, 0};
		double[] chi = new double[] {Double.MAX_VALUE, Double.MAX_VALUE,
				Double.MAX_VALUE, Double.MAX_VALUE};
		DenseDoubleMatrix2D a = new DenseDoubleMatrix2D(ca);
		EigenvalueDecomposition ed = new EigenvalueDecomposition(a);
		DenseDoubleMatrix1D b = new DenseDoubleMatrix1D(cb);
		DenseDoubleMatrix1D lo = new DenseDoubleMatrix1D(clo);
		DenseDoubleMatrix1D hi = new DenseDoubleMatrix1D(chi);
		int maxIteration = 1000000;
		double threshold = util.Constants.FP_TOLERANCE;
		PGSResult result = psg.solve(a, b, lo, hi, maxIteration, threshold, true);
		DenseDoubleMatrix1D w = new DenseDoubleMatrix1D(4);
		a.zMult(result.result, w);
		w.assign(b, Functions.plus);
		for (int i = 0; i < 4; i++)
		{
			assert(Math.abs(w.get(i)) < 1e-8);
//			if (w.get(i) > 0)
//				assert(result.result.get(i) == 0);
		}
	}
	
	private double absDiff(double a, double b)
	{
		return Math.abs(a - b);
	}
	
	private void testOBBIntersection()
	{
		OBB obb1 = new OBB();
		obb1.center = new Point3d(-2, 0, 0);
		obb1.size[0] = 1;
		obb1.size[1] = 1;
		obb1.size[2] = 1;
		obb1.axes[0] = new Vector3d(1, 0, 0);
		obb1.axes[1] = new Vector3d(0, 1, 0);
		obb1.axes[2] = new Vector3d(0, 0, 1);
		
		OBB obb2 = new OBB();
		obb2.center = new Point3d(2, 0, 0);
		obb2.size[0] = 1;
		obb2.size[1] = 1;
		obb2.size[2] = 1;
		obb2.axes[0] = new Vector3d(1, 0, 0);
		obb2.axes[1] = new Vector3d(0, 1, 0);
		obb2.axes[2] = new Vector3d(0, 0, 1);
		
		Matrix4d transform1 = new Matrix4d();
		transform1.setIdentity();
		Matrix4d transform2 = new Matrix4d();
		transform2.setIdentity();
		Matrix4d transform1inv = new Matrix4d(transform1);
		transform1inv.invert();
		boolean isColliding = OBB.isCollidingSlow(obb1, transform1inv, obb2, transform2);
	}
	
	private void testBoxIntersectWithRay()
	{
		BoxRigidBody body = new BoxRigidBody(0, new Point3d(0, 0, 0), new Vector3d(1, 1, 1));
		Point3d p0 = new Point3d(-1, -1, -1);
		Point3d p1 = new Point3d(1, 1, 1);
		Point3d p = body.intersectWithRay(p0, p1, body.getTransformMatrixInverse());
	}

	private void testContactGraph()
	{
		ContactGraph contactGraph = new ContactGraph();
		
		int objCount = 10;
		RigidBody[] b = new RigidBody[objCount];
		for (int i = 0; i < objCount; i++)
		{
			b[i] = new RigidBody(1, new Point3d(0, 0, 0), new Box(1, 1, 1, new Color3f()));
			b[i].setId(i);
		}
		
		try
		{
			contactGraph.init(Arrays.asList(b), 0.01);
		} catch (Exception e)
		{
			e.printStackTrace();
		}
		
		List<RigidBody>[] graph = contactGraph.getGraph();
		graph[0].add(b[1]);
		graph[1].add(b[2]);
		graph[2].add(b[3]);
		graph[3].add(b[1]);
		graph[9].add(b[8]);
		graph[8].add(b[2]);
		graph[2].add(b[9]);
		graph[4].add(b[5]);
		graph[5].add(b[6]);
		graph[6].add(b[4]);
		graph[7].add(b[5]);
		graph[7].add(b[6]);
		
		contactGraph.updateOrder();
	}

	private void testASDF()
	{
		RigidBody boxBody1 = new BoxRigidBody(0.2, new Point3d(0, 0, 0), new Vector3d(1, 1, 1));
		Point3d targetPoint = new Point3d();
		Vector3d normal = new Vector3d();
		boxBody1.getASDFTree().getDistance(new Point3d(0, 0, 0), normal);
		boxBody1.getASDFTree().getDistance(new Point3d(0.9, 0, 0), normal);
		boxBody1.getASDFTree().getDistance(new Point3d(0.49, 0, 0), normal);
		boxBody1.getASDFTree().getDistance(new Point3d(0.48, 0, 0), normal);
		boxBody1.getASDFTree().getDistance(new Point3d(0.47, 0, 0), normal);
		boxBody1.getASDFTree().getDistance(new Point3d(0.47, 0.49, 0), normal);
		boxBody1.getASDFTree().getDistance(new Point3d(0.47, 0.49, 0.3), normal);
		boxBody1.getASDFTree().getDistance(new Point3d(0.9, 0, 0), normal);
		boxBody1.getASDFTree().getDistance(new Point3d(0.4, 0, 0), normal);
		boxBody1.getASDFTree().getDistance(new Point3d(0.3, 0.49, 0), normal);
	}
	
	private void testTriangleDistance()
	{
		Triangle triangle = new Triangle(new Point3d(0, 0, 0), new Point3d(1, 0, 0), new Point3d(0, 1, 0),
										new Color3f(0.7f, 0.7f, 1), null);
		Point3d p = new Point3d(0, 0, 0);
		Point3d targetPoint = new Point3d();
		
//		double t = triangle.getClosestPoint(p, targetPoint);
//		
//		p = new Point3d(0.5, 0.5, 0);
//		t = triangle.getClosestPoint(p, targetPoint);
//		
//		p = new Point3d(0.5, 0.5, 1);
//		t = triangle.getClosestPoint(p, targetPoint);
//		
//		p = new Point3d(0.5, 0.5, -1);
//		t = triangle.getClosestPoint(p, targetPoint);
//		
//		p = new Point3d(0.5, -1, -1);
//		t = triangle.getClosestPoint(p, targetPoint);
	}
	
	private void testPenetrationDetection()
	{
		RigidBody box = new BoxRigidBody(0.2, new Point3d(0, 0, 0), new Vector3d(1, 1, 1));
		Point3d p = new Point3d(0, 0, 0);
		Point3d targetPoint = new Point3d();
		Triangle triangle = new Triangle();
		double d = box.getTriangleMesh().distanceToPoint(p, triangle, targetPoint);
		
		p = new Point3d(1, 1, 1);
		d = box.getTriangleMesh().distanceToPoint(p, triangle, targetPoint);
		
		p = new Point3d(0.5, 0.5, -1);
		d = box.getTriangleMesh().distanceToPoint(p, triangle, targetPoint);
		
		p = new Point3d(0.5, 0.5, -0.5);
		d = box.getTriangleMesh().distanceToPoint(p, triangle, targetPoint);
		
		p = new Point3d(1, 1, -2);
		d = box.getTriangleMesh().distanceToPoint(p, triangle, targetPoint);
		
		for (int i = 0; i < 1000; i++)
		{
			double x = Math.random();
			double y = Math.random();
			double z = -Math.random();
			p = new Point3d(x, y, z);
			d = box.getTriangleMesh().distanceToPoint(p, triangle, targetPoint);
			double ans = Double.POSITIVE_INFINITY;
			ans = Math.min(ans, x);
			ans = Math.min(ans, 1 - x);
			ans = Math.min(ans, y);
			ans = Math.min(ans, 1 - y);
			ans = Math.min(ans, -z);
			ans = Math.min(ans, 1 + z);
			if (d != ans)
				throw new RuntimeException();
		}
		
		for (int i = 0; i < 1000; i++)
		{
			double x = Math.random() * 100 * (Math.random() - 0.5);
			double y = Math.random() * 100 * (Math.random() - 0.5);
			double z = Math.random() * 100 * (Math.random() - 0.5);
			while ((0 < x && x < 1) || (0 < y && y < 1) || (-1 < z && z < 0))
			{
				x = Math.random() * 100 * (Math.random() - 0.5);
				y = Math.random() * 100 * (Math.random() - 0.5);
				z = Math.random() * 100 * (Math.random() - 0.5);
			}
			double ans = Double.POSITIVE_INFINITY;
			p = new Point3d(x, y, z);
			ans = Math.min(ans, Util.distance(p, new Point3d(0, 0, 0)));
			ans = Math.min(ans, Util.distance(p, new Point3d(0, 1, 0)));
			ans = Math.min(ans, Util.distance(p, new Point3d(1, 0, 0)));
			ans = Math.min(ans, Util.distance(p, new Point3d(1, 1, 0)));
			ans = Math.min(ans, Util.distance(p, new Point3d(0, 0, -1)));
			ans = Math.min(ans, Util.distance(p, new Point3d(0, 1, -1)));
			ans = Math.min(ans, Util.distance(p, new Point3d(1, 0, -1)));
			ans = Math.min(ans, Util.distance(p, new Point3d(1, 1, -1)));
			d = box.getTriangleMesh().distanceToPoint(p, triangle, targetPoint);
			if (Math.abs(d - ans) > 1e-8)
				throw new RuntimeException();
		}
	}
	
	private void testInsideCheck()
	{
		RigidBody box = new BoxRigidBody(0.2, new Point3d(0, 0, 0), new Vector3d(1, 1, 1));
		Point3d p = new Point3d(0, 0, 0);
		boolean in = box.getTriangleMesh().isInside(p);
		
		p = new Point3d(1, 1, 1);
		in = box.getTriangleMesh().isInside(p);
		
		p = new Point3d(0.5, 0.5, -1);
		in = box.getTriangleMesh().isInside(p);
		
		p = new Point3d(0.5, 0.5, -0.5);
		in = box.getTriangleMesh().isInside(p);
		
		p = new Point3d(1, 1, -2);
		in = box.getTriangleMesh().isInside(p);
		
		for (int i = 0; i < 1000; i++)
		{
			double x = Math.random();
			double y = Math.random();
			double z = -Math.random();
			p = new Point3d(x, y, z);
			in = box.getTriangleMesh().isInside(p);
			if (in == false)
				throw new RuntimeException();
		}
		
		for (int i = 0; i < 1000; i++)
		{
			double x = Math.random() * 100 * (Math.random() - 0.5);
			double y = Math.random() * 100 * (Math.random() - 0.5);
			double z = Math.random() * 100 * (Math.random() - 0.5);
			while ((0 < x && x < 1) && (0 < y && y < 1) && (-1 < z && z < 0))
			{
				x = Math.random() * 100 * (Math.random() - 0.5);
				y = Math.random() * 100 * (Math.random() - 0.5);
				z = Math.random() * 100 * (Math.random() - 0.5);
			}
			p = new Point3d(x, y, z);
			in = box.getTriangleMesh().isInside(p);
			if (in == true)
				throw new RuntimeException();
		}
	}
}
